#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <cmath>
#include <string>
#include <Eigen/Eigen>
#include "visualization_msgs/Marker.h"

using namespace std;

Eigen::Vector3d target_wp{5.2, -3.3, 0.0};

void poi_callback(const geometry_msgs::PoseStampedConstPtr &msg){
    target_wp[0]=msg->pose.position.x;
    target_wp[1]=msg->pose.position.y;
    target_wp[2]=msg->pose.position.z;
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "up_planner_node");
    ros::NodeHandle nh("~");

    // useless
    float map_x_size_ = 30.0;
    float map_y_size_ = 30.0;
    float map_z_size_ = 5.0;

    ros::Publisher moving_target_pub = nh.advertise<visualization_msgs::Marker>("/moving", 10);
    ros::Subscriber moving_target_sub = nh.subscribe<geometry_msgs::PoseStamped>("/drone_0/moving_goal", 10,poi_callback);

    visualization_msgs::Marker marker_;
    marker_.ns = "my_namespace";
    marker_.header.stamp = ros::Time();
    marker_.header.frame_id = "world";
    marker_.type = visualization_msgs::Marker::SPHERE;
    marker_.scale.x = 0.5;
    marker_.scale.y = 0.5;
    marker_.scale.z = 0.5;
    /*decide the color of the marker*/
    marker_.color.a = 1.0; // Don't forget to set the alpha!
    marker_.color.r = 0.8;
    marker_.color.g = 0;
    marker_.color.b = 0;
    /*set marker action*/
    marker_.action = visualization_msgs::Marker::ADD;

    ros::Rate rate(1000);

    cout << "go!" << endl;
    int count = 0;
    while (ros::ok())
    {
        marker_.pose.position.x = target_wp(0);
        marker_.pose.position.y = target_wp(1);
        marker_.pose.position.z = 0.0;
        moving_target_pub.publish(marker_);
        // cout<<target[0]<<"          "<<target[1]<<endl;
        rate.sleep();
    }

    return 0;
}